#include "pch.h"

#include "Mathematics.h"

Vector3f &Vector3f::rotate(fp32 angle, const Vector3f &axis) {
	const fp32 sin_half_angle		= sinf(TO_RADIAN(angle / 2));
	const fp32 cos_half_angle		= cosf(TO_RADIAN(angle / 2));

	const fp32 r_x					= axis.x() * sin_half_angle;
	const fp32 r_y					= axis.y() * sin_half_angle;
	const fp32 r_z					= axis.z() * sin_half_angle;
	const fp32 r_w					= cos_half_angle;
	Quaternion rotation_q			(r_x, r_y, r_z, r_w);

	Quaternion conjugate_q			= rotation_q.conjugate();
//	ConjugateQ.Normalize();
	Quaternion w					= rotation_q * (*this) * conjugate_q;

	x								(w.x());
	y								(w.y());
	z								(w.z());

	return							(*this);
}
